/*
 *  Controller.c
 *  For the UNM Neural Networks class, this is the only fle you will need to modify.
 *  It contains the agent initialization code and the agent controller code.  An
 *  example of a non-neural controller is included here, which can be modified.
 *  Note that most all of the functions called here can be found in the 
 *  file FlatworldIICore.c
 *  
 *
 *  Created by Thomas Caudell on 9/15/09.
 *  Modified by Thomas Caudell on 9/30/2010
 *  Modified by Thomas Caudell on 9/13/2012
 *  Copyright 2009 UNM. All rights reserved.
 *
 */

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <jni.h>

#include "FlatworldIICore.h"
#include "Distributions_Funcs.h"
#include "Controller.h"

void runMode(WORLD_TYPE *w);
void trainMode(WORLD_TYPE *w);
double spiralSpeed = 0.225;
void agents_controller(WORLD_TYPE *w) { /* Adhoc function to test agents, to be replaced with NN controller. tpc */

//	trainMode(w);
	runMode(w);

}

void trainMode(WORLD_TYPE *w) {

	AGENT_TYPE *a;
	int collision_flag = 0;
	int i, j, k, index;
	int maxvisualreceptor = -1;
	int nsomareceptors;
	int nacousticfrequencies;
	double max;

	float delta_energy;
	float dfb, drl, dth, dh;
	float headth;
	float forwardspeed;
	float maxvisualreceptordirection;
	float bodyx, bodyy, bodyth;
	float x, y, h;
	float **eyevalues, **ear0values, **ear1values, **skinvalues;
	float ear0mag = 0.0, ear1mag = 0.0;
	time_t now;
	struct tm *date;
	char timestamp[30];
	int deathTime;
	jclass clazz;
	jmethodID id;
	jdouble eyeAnswer;

	/* Initialize parameters */
	forwardspeed = 0.09;

	a = w->agents[0]; /* get agent pointer */

	/* test if agent is alive. if so, process sensors and actuators.  if not, report death and exit */
	if (a->instate->metabolic_charge > 0.0) {
		/* get current muscle rates and body/head angles */
		read_actuators_agent(a, &dfb, &drl, &dth, &dh);
		read_agent_body_position(a, &bodyx, &bodyy, &bodyth);
		read_agent_head_angle(a, &headth);

		/* read visual sensor to get R, G, B intensity values */
		read_visual_sensor(w, a);
		eyevalues = extract_visual_receptor_values_pointer(a, 0);

		//read eyelets, determine angle to turn towards

		/* read somatic(touch) sensor for collision */
		collision_flag = read_soma_sensor(w, a);
		skinvalues = extract_soma_receptor_values_pointer(a);
		nsomareceptors = get_number_of_soma_receptors(a);

		if (skinvalues[0][0] > 0.0) {

			delta_energy = eat_colliding_object(w, a, 0);

			int result = 0;
			if (delta_energy > 0)
				result = 1;

			clazz = (*jniENV)->FindClass(jniENV, "FlatWorld");
			id = (*jniENV)->GetMethodID(jniENV, clazz, "trainEye", "(DDDI)V");

			(*jniENV)->CallVoidMethod(jniENV, object, id, eyevalues[15][0],
					eyevalues[15][1], eyevalues[15][2], result);
		}

		/* read hearing sensors and load spectra for each ear, and compute integrated sound magnitudes */
//		read_acoustic_sensor(w, a);
//		ear0values = extract_sound_receptor_values_pointer(a, 0);
//		ear1values = extract_sound_receptor_values_pointer(a, 1);
//		nacousticfrequencies = get_number_of_acoustic_receptors(a);
//		for (i = 0; i < nacousticfrequencies; i++) {
//			ear0mag += ear0values[i][0];
//			ear1mag += ear1values[i][0];
//		}
		//printf("ear0mag: %f ear1mag: %f\n",ear0mag,ear1mag) ;
		/* find brights object in visual field */
		maxvisualreceptor = intensity_winner_takes_all(a);
		if (maxvisualreceptor >= 0) {
			/* use brightest visual receptor to determine how to turn body to center it in the field of view */
			maxvisualreceptordirection = visual_receptor_position(
					a->instate->eyes[0], maxvisualreceptor);
//
//			/* rotate body to face brightes object */
			set_agent_body_angle(a, bodyth + maxvisualreceptordirection);
		}
//      else
//		{
//			printf(
//					"agents_controller-  No visible object, simtime: %d, changing direction.\n",
//					simtime);
//			read_agent_body_position(a, &bodyx, &bodyy, &bodyth);
//			set_agent_body_angle(a, bodyth + 45.0);
//
//		}
		/* move the agents body */
		set_forward_speed_agent(a, forwardspeed);
		move_body_agent(a);

		/* decrement metabolic charge by basil metabolism rate.  DO NOT REMOVE THIS CALL */
		basal_metabolism_agent(a);

		/* increment world time clock */
		increment_world_clock(w);

	} /* end agent alive condition */
	else {

		/* Example of agent is dead condition */
//		printf(
//				"agent_controller- Agent has died, eating %d objects. simtime: %d\n",
//				a->instate->itemp[0], simtime);
		print_world_time(Flatworld);
		now = time(NULL );
		date = localtime(&now);
		strftime(timestamp, 30, "%y/%m/%d H: %H M: %M S: %S", date);
//		printf("Death time: %s\n", timestamp);

		clazz = (*jniENV)->FindClass(jniENV, "FlatWorld");
		id = (*jniENV)->GetMethodID(jniENV, clazz, "recordDeathTime", "(I)V");
		(*jniENV)->CallVoidMethod(jniENV, object, id, simtime);

		clazz = (*jniENV)->FindClass(jniENV, "FlatWorld");
		id = (*jniENV)->GetMethodID(jniENV, clazz, "recordObjectEaten", "(I)V");
		(*jniENV)->CallVoidMethod(jniENV, object, id, a->instate->itemp[0]);

		simtime = 0;

		/* Example as to how to restore the world and agent after it dies. */
		restore_objects_to_world(Flatworld); /* restore all of the objects back into the world */
		reset_agent_charge(a); /* recharge the agent's battery to full */
		a->instate->itemp[0] = 0; /* zero the number of object's eaten accumulator */
		init_world_time(Flatworld); /* zero the Flatworld clock */
		x = distributions_uniform(Flatworld->xmin, Flatworld->xmax); /* pick random starting position and heading */
//		x = distributions_uniform( Flatworld->xmin, Flatworld->xmax ) ; /* pick random starting position and heading */
		y = distributions_uniform(Flatworld->ymin, Flatworld->ymax);
		h = distributions_uniform(-179.0, 179.0);
//		printf(
//				"\nagent_controller- new coordinates after restoration:  x: %f y: %f h: %f\n",
//				x, y, h);
		set_agent_body_position(a, x, y, h); /* set new position and heading of agent */

	} /* end agent dead condition */

}

void runMode(WORLD_TYPE *w) {

	AGENT_TYPE *a;
	int collision_flag = 0;
	int i, j, k, index;
	int maxvisualreceptor = -1;
	int nsomareceptors;
	int nacousticfrequencies;
	double max;

	float delta_energy;
	float dfb, drl, dth, dh;
	float headth;
	float forwardspeed;
	float maxvisualreceptordirection;
	float bodyx, bodyy, bodyth;
	float x, y, h;
	float **eyevalues, **ear0values, **ear1values, **skinvalues;
	float ear0mag = 0.0, ear1mag = 0.0;
	time_t now;
	struct tm *date;
	char timestamp[30];
	int deathTime;
	jclass clazz;
	jmethodID id;
	jdouble eyeAnswer;

	//current eyelet angles
	float angle_locations0[31] = { -15., -14., -13., -12., -11., -10., -9., -8.,
			-7., -6., -5., -4., -3., -2., -1., 0., 1., 2., 3., 4., 5., 6., 7.,
			8., 9., 10., 11., 12., 13., 14., 15 };

	/* Initialize parameters */
	forwardspeed = 0.125;

	a = w->agents[0]; /* get agent pointer */

	/* test if agent is alive. if so, process sensors and actuators.  if not, report death and exit */
	if (a->instate->metabolic_charge > 0.0) {
		/* get current muscle rates and body/head angles */
		read_actuators_agent(a, &dfb, &drl, &dth, &dh);
		read_agent_body_position(a, &bodyx, &bodyy, &bodyth);
		read_agent_head_angle(a, &headth);

		/* read visual sensor to get R, G, B intensity values */
		read_visual_sensor(w, a);
		eyevalues = extract_visual_receptor_values_pointer(a, 0);

		//read eyelets, determine angle to turn towards
		max = 0.0;
		for (i = 0; i < 31; i++) {

			clazz = (*jniENV)->FindClass(jniENV, "FlatWorld");
			id = (*jniENV)->GetMethodID(jniENV, clazz, "useEye", "(DDD)D");

			eyeAnswer = (*jniENV)->CallDoubleMethod(jniENV, object, id,
					eyevalues[i][0], eyevalues[i][1], eyevalues[i][2]);

			if (eyeAnswer > max) {
				max = eyeAnswer;
				index = i;
			}
		}

		//printf("\n\nMax: %g\n\n", max);
		if (max > 0) {
			spiralSpeed = .225;
			maxvisualreceptordirection = visual_receptor_position(
					a->instate->eyes[0], index);

			//printf("Arg Max Index %d, angle: %g, eyeAnswer: %g", index,
			//		maxvisualreceptordirection, eyeAnswer);

			set_agent_body_angle(a, bodyth + maxvisualreceptordirection);
			set_forward_speed_agent(a, forwardspeed);
		} else {
			set_agent_body_angle(a, bodyth + 30);
			set_forward_speed_agent(a, spiralSpeed);
			spiralSpeed = spiralSpeed + .002;
		}

		/* read somatic(touch) sensor for collision */
		collision_flag = read_soma_sensor(w, a);
		skinvalues = extract_soma_receptor_values_pointer(a);
		nsomareceptors = get_number_of_soma_receptors(a);
		for (k = 0; k < nsomareceptors; k++) {
			if (k == 0 && skinvalues[k][0] > 0.0) {

//				printf("Eye: %d,\n\t", 15);
//				for (j = 0; j < a->instate->eyes[0]->nbands; j++) {
//					printf("%g, ", eyevalues[15][j]);
//				}
//				printf("\n");

				eyeAnswer = (*jniENV)->CallDoubleMethod(jniENV, object, id,
						eyevalues[15][0], eyevalues[15][1], eyevalues[15][2]);
				if (eyeAnswer > 0.3) {
					if (a->instate->metabolic_charge < 0.9)
					{
						delta_energy = eat_colliding_object(w, a, k);

						if (delta_energy < 0)
							printf("Just Ate Poisen");

					}
					else
						set_forward_speed_agent(a, 0.0);
				}

			}
		}

		/* read hearing sensors and load spectra for each ear, and compute integrated sound magnitudes */
//		read_acoustic_sensor(w, a);
//		ear0values = extract_sound_receptor_values_pointer(a, 0);
//		ear1values = extract_sound_receptor_values_pointer(a, 1);
//		nacousticfrequencies = get_number_of_acoustic_receptors(a);
//		for (i = 0; i < nacousticfrequencies; i++) {
//			ear0mag += ear0values[i][0];
//			ear1mag += ear1values[i][0];
//		}
		//printf("ear0mag: %f ear1mag: %f\n",ear0mag,ear1mag) ;
		/* find brights object in visual field */
	//	maxvisualreceptor = intensity_winner_takes_all(a);
	//	if (maxvisualreceptor >= 0) {
			/* use brightest visual receptor to determine how to turn body to center it in the field of view */
	//		maxvisualreceptordirection = visual_receptor_position(
	//				a->instate->eyes[0], maxvisualreceptor);

			/* rotate body to face brightes object */
	//		set_agent_body_angle(a, bodyth + maxvisualreceptordirection);
	//	}

//      else
//		{
//			printf(
//					"agents_controller-  No visible object, simtime: %d, changing direction.\n",
//					simtime);
//			read_agent_body_position(a, &bodyx, &bodyy, &bodyth);
//			set_agent_body_angle(a, bodyth + 45.0);
//
//		}
		/* move the agents body */

		move_body_agent(a);

		/* decrement metabolic charge by basil metabolism rate.  DO NOT REMOVE THIS CALL */
		basal_metabolism_agent(a);

		/* increment world time clock */
		increment_world_clock(w);

	} /* end agent alive condition */
	else {

		/* Example of agent is dead condition */
//		printf(
//				"agent_controller- Agent has died, eating %d objects. simtime: %d\n",
//				a->instate->itemp[0], simtime);
		spiralSpeed = 0.225;
		print_world_time(Flatworld);
		now = time(NULL );
		date = localtime(&now);
		strftime(timestamp, 30, "%y/%m/%d H: %H M: %M S: %S", date);
//		printf("Death time: %s\n", timestamp);

		clazz = (*jniENV)->FindClass(jniENV, "FlatWorld");
		id = (*jniENV)->GetMethodID(jniENV, clazz, "recordDeathTime", "(I)V");
		(*jniENV)->CallVoidMethod(jniENV, object, id, simtime);

		clazz = (*jniENV)->FindClass(jniENV, "FlatWorld");
		id = (*jniENV)->GetMethodID(jniENV, clazz, "recordObjectEaten", "(I)V");
		(*jniENV)->CallVoidMethod(jniENV, object, id, a->instate->itemp[0]);

		simtime = 0;

		/* Example as to how to restore the world and agent after it dies. */
		restore_objects_to_world(Flatworld); /* restore all of the objects back into the world */
		reset_agent_charge(a); /* recharge the agent's battery to full */
		a->instate->itemp[0] = 0; /* zero the number of object's eaten accumulator */
		init_world_time(Flatworld); /* zero the Flatworld clock */
		x = distributions_uniform(Flatworld->xmin, Flatworld->xmax); /* pick random starting position and heading */
//		x = distributions_uniform( Flatworld->xmin, Flatworld->xmax ) ; /* pick random starting position and heading */
		y = distributions_uniform(Flatworld->ymin, Flatworld->ymax);
		h = distributions_uniform(-179.0, 179.0);
//		printf(
//				"\nagent_controller- new coordinates after restoration:  x: %f y: %f h: %f\n",
//				x, y, h);
		set_agent_body_position(a, x, y, h); /* set new position and heading of agent */

	} /* end agent dead condition */
}

